#ifdef KITSCHY_DEBUG_MEMORY 
#include "debug_memorymanager.h"
#endif

#ifdef WIN32
#include "windows.h"
#else
#include <sys/time.h>
#include <time.h>
#endif
#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "stdarg.h"
#include "math.h"
#include "stdlib.h"

#include "BList.h"
#include "Pair.h"

#include "symbol.h"
#include "sort.h"
#include "ontology.h"
#include "featureterm.h"
#include "FTKBase.h"
#include "AUfeatureterm.h"
#include "Ufeatureterm.h"
#include "FT_refinement.h"
#include "FT_generalization.h"

#include "Planning.h"
#include "SNLP.h"

List<FeatureTerm> *SNLP_step1(SNLPState *state,FTKBase *dm,Ontology *o);
List<FeatureTerm> *SNLP_step2(SNLPState *state,FTKBase *dm,Ontology *o);

List<SNLPState> *SNLP_getSuccessors_step1(SNLPState *state,FTKBase *dm,Ontology *o);
List<SNLPState> *SNLP_getSuccessors_step2(SNLPState *state,FTKBase *dm,Ontology *o);

List<SNLPState> *SNLP_get_potentially_existing_satisfying_operators(SNLPState *state,Pair<FeatureTerm,FeatureTerm> *g,FTKBase *dm,Ontology *o);
List<SNLPState> *SNLP_get_potentially_new_satisfying_operators(SNLPState *state,FeatureTerm *g,FTKBase *dm,Ontology *o);

bool HSNLPSortOrder(SNLPState *n1,SNLPState *n2);



// Implement UCPOP (which is likely what I'll need for my GDL system)
// Check this out: http://www.cs.cmu.edu/~reids/planning/handouts/Partial_Order.pdf
// ...


List<FeatureTerm> *SNLP_dfs(FeatureTerm *initialState,FeatureTerm *goalState,FTKBase *dm,Ontology *o)
{    
	SNLPState *initial_problem = new SNLPState();
    
    // Create basic operators (start and end):
    {
        FeatureTerm *op1 = FeatureTerm::fromStringNOOS("(define (operator) (name \"initial-state\"))",dm,o);
        op1->defineFeatureValue("effect",new FeatureTerm(initialState));
        FeatureTerm *op2 = FeatureTerm::fromStringNOOS("(define (operator) (name \"goal-state\"))",dm,o);
        op2->defineFeatureValue("precondition",new FeatureTerm(goalState));
        
        initial_problem->operators.Add(op1);
        initial_problem->operators.Add(op2);
        
        if (goalState->getSort()->is_a(and_sort)) {
            FeatureTerm *f = goalState->featureValueQuick("predicates");
            if (f->isSet()) {
                List<FeatureTerm> l;
                l.Instance(*(f->set_values_quick()));
                l.Rewind();
                while(l.Iterate(f)) {
                    initial_problem->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(new FeatureTerm(f),new FeatureTerm(op2)));
                } // while
            } else {
                initial_problem->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(new FeatureTerm(f),new FeatureTerm(op2)));
            } // if
        } else {
            printf("SNLP only supports 'and' goal state structures!\n");
            return 0;
        } // if
    }
    
    //	List<FeatureTerm> *plan = SNLP_step1(initial_problem,dm,o);
    
    {
        List<SNLPState> open,*next;
        SNLPState *state;
        
        open.Add(initial_problem);
        
        while(!open.EmptyP()) {
            
            state = open.ExtractIni();
            
            printf("[%i open conds, %i ops] - %i open nodes\n",state->open_conditions.Length(),state->operators.Length(),open.Length());
            
            if (state->open_conditions.EmptyP()) {
                List<FeatureTerm> *plan = state->get_linearization();
                delete state;
                return plan;
            } else {
                next = SNLP_getSuccessors_step1(state,dm,o);
                while(!next->EmptyP()) open.Insert(next->Extract());
                delete next;
                delete state;
            }
        }
    }
    
	return 0;
} /* SNLP_dfs */ 



List<FeatureTerm> *SNLP_bfs(FeatureTerm *initialState,FeatureTerm *goalState,FTKBase *dm,Ontology *o)
{    
	SNLPState *initial_problem = new SNLPState();
    
    // Create basic operators (start and end):
    {
        FeatureTerm *op1 = FeatureTerm::fromStringNOOS("(define (operator) (name \"initial-state\"))",dm,o);
        op1->defineFeatureValue("effect",new FeatureTerm(initialState));
        FeatureTerm *op2 = FeatureTerm::fromStringNOOS("(define (operator) (name \"goal-state\"))",dm,o);
        op2->defineFeatureValue("precondition",new FeatureTerm(goalState));
        
        initial_problem->operators.Add(op1);
        initial_problem->operators.Add(op2);
        
        if (goalState->getSort()->is_a(and_sort)) {
            FeatureTerm *f = goalState->featureValueQuick("predicates");
            if (f->isSet()) {
                List<FeatureTerm> l;
                l.Instance(*(f->set_values_quick()));
                l.Rewind();
                while(l.Iterate(f)) {
                    initial_problem->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(new FeatureTerm(f),new FeatureTerm(op2)));
                } // while
            } else {
                initial_problem->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(new FeatureTerm(f),new FeatureTerm(op2)));
            } // if
        } else {
            printf("SNLP only supports 'and' goal state structures!\n");
            return 0;
        } // if
    }
    
//	List<FeatureTerm> *plan = SNLP_step1(initial_problem,dm,o);
  
    {
        List<SNLPState> open,*next;
        SNLPState *state;
        
        open.Add(initial_problem);
        
        while(!open.EmptyP()) {
                        
            state = open.ExtractIni();
            
            printf("[%i open conds, %i ops] - %i open nodes\n",state->open_conditions.Length(),state->operators.Length(),open.Length());

            
            if (state->open_conditions.EmptyP()) {
                List<FeatureTerm> *plan = state->get_linearization();
                delete state;
                return plan;
            } else {
                next = SNLP_getSuccessors_step1(state,dm,o);
                open.AddAll(next);
                next->ExtractAll();
                delete next;
                delete state;
            }
        }
    }
    
	return 0;
} /* SNLP_bfs */ 


List<FeatureTerm> *SNLP_h(FeatureTerm *initialState,FeatureTerm *goalState,FTKBase *dm,Ontology *o)
{    
	SNLPState *initial_problem = new SNLPState();
    
    // Create basic operators (start and end):
    {
        FeatureTerm *op1 = FeatureTerm::fromStringNOOS("(define (operator) (name \"initial-state\"))",dm,o);
        op1->defineFeatureValue("effect",new FeatureTerm(initialState));
        FeatureTerm *op2 = FeatureTerm::fromStringNOOS("(define (operator) (name \"goal-state\"))",dm,o);
        op2->defineFeatureValue("precondition",new FeatureTerm(goalState));
        
        initial_problem->operators.Add(op1);
        initial_problem->operators.Add(op2);
        
        if (goalState->getSort()->is_a(and_sort)) {
            FeatureTerm *f = goalState->featureValueQuick("predicates");
            if (f->isSet()) {
                List<FeatureTerm> l;
                l.Instance(*(f->set_values_quick()));
                l.Rewind();
                while(l.Iterate(f)) {
                    initial_problem->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(new FeatureTerm(f),new FeatureTerm(op2)));
                } // while
            } else {
                initial_problem->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(new FeatureTerm(f),new FeatureTerm(op2)));
            } // if
        } else {
            printf("SNLP only supports 'and' goal state structures!\n");
            return 0;
        } // if
    }
    
    //	List<FeatureTerm> *plan = SNLP_step1(initial_problem,dm,o);
    
    {
        List<SNLPState> open,*next;
        SNLPState *state;
        
        open.Add(initial_problem);
        
        while(!open.EmptyP()) {
            
            open.Sort(&HSNLPSortOrder);
            
            state = open.ExtractIni();
            
            printf("[%i open conds, %i ops] - %i open nodes\n",state->open_conditions.Length(),state->operators.Length(),open.Length());
            
            
            if (state->open_conditions.EmptyP()) {
                List<FeatureTerm> *plan = state->get_linearization();
                delete state;
                return plan;
            } else {
                next = SNLP_getSuccessors_step1(state,dm,o);
                open.AddAll(next);
                next->ExtractAll();
                delete next;
                delete state;
            }
        }
    }
    
	return 0;
} /* SNLP_h */ 



List<SNLPState> *SNLP_getSuccessors_step1(SNLPState *state,FTKBase *dm,Ontology *o) 
{
    List<SNLPState> *successors = new List<SNLPState>,*successors_tmp;
    
//    printf("\SNLP_getSuccessors_step1 ***********************************************\n");
//    state->printStateNice(dm);
    
    Pair<FeatureTerm,FeatureTerm> *s_add_existing;
    FeatureTerm *s_add;
    Pair<FeatureTerm,FeatureTerm> *g;
    List<Pair<FeatureTerm,FeatureTerm> > l;
    List<FeatureTerm> *plan;
    l.Instance(state->open_conditions);
    l.Rewind();
    while(l.Iterate(g)) {
        
/*        {
            char *s = g->m_a->toStringNOOS(dm);
            printf("SNLP_step1: Attempting:\n%s\n",s);
            delete []s;
        }
*/        
        List<SNLPState> *l2 = SNLP_get_potentially_existing_satisfying_operators(state,g,dm,o);
        SNLPState *state2;
        
        l2->Rewind();
        while(l2->Iterate(state2)) {
            successors_tmp = SNLP_getSuccessors_step2(state2,dm,o);
//            printf("[%i]",successors_tmp->Length());
            successors->AddAll(successors_tmp);
            successors_tmp->ExtractAll();
            delete successors_tmp;
        } // while
        delete l2;
        
        l2 = SNLP_get_potentially_new_satisfying_operators(state,g->m_a,dm,o);
        
        l2->Rewind();
        while(l2->Iterate(state2)) {
            successors_tmp = SNLP_getSuccessors_step2(state2,dm,o);
//            printf("{%i}",successors_tmp->Length());
            successors->AddAll(successors_tmp);
            successors_tmp->ExtractAll();
            delete successors_tmp;
        } // while
        delete l2;
    } // while
    
    printf("%i successors\n",successors->Length());
    return successors;
} /* SNLP_getSuccessors */ 


List<SNLPState> *SNLP_getSuccessors_step2(SNLPState *state,FTKBase *dm,Ontology *o)
{
    List<SNLPState> *successors = new List<SNLPState>,*successors_tmp;

//    printf("SNLP_getSuccessors_step2\n");
//    state->printStateNice(dm);
    
	// Get all the threats:
    SNLPThreat *threat = 0;
    
	SNLPCausalLink *cl;
    List<SNLPCausalLink> l;
    FeatureTerm *op;
    List<FeatureTerm> l2;
    l.Instance(state->causal_links);
    l.Rewind();
    l2.Instance(state->operators);
    while(threat == 0 && l.Iterate(cl)) {
        if (cl->condition!=0) {
            l2.Rewind();
            while(threat == 0 && l2.Iterate(op)) {
                //                printf("SNLP_step2: considering %i ...\n",state->operators.Position(op));
                if (!op->same(cl->op1) &&
                    !op->same(cl->op2)) {
                    
                    //                    printf("not one of them...\n");
                    
                    if (!state->operator_before(op,cl->op1) &&
                        !state->operator_before(cl->op2,op)) {
                        // Operator might happen in between!
                        // Check if any of the effects of op can unify with cl->g
                        FeatureTerm *effect = op->featureValueQuick("effect");
                        //                        printf("effect sort 1: %s\n",effect->getSort()->get());
                        if (effect->getSort()==and_sort) {
                            
                            effect = effect->featureValueQuick("predicates");
                            if (effect->isSet()) {
                                List<FeatureTerm> l;
                                l.Instance(*(effect->set_values_quick()));
                                l.Rewind();
                                while(threat==0 && l.Iterate(effect)) {
                                    //                                    printf("effect sort 2a: %s\n",effect->getSort()->get());
                                    if (effect->getSort()==not_sort) effect = effect->featureValueQuick("predicate");
                                    //                                    printf("effect sort 2b: %s\n",effect->getSort()->get());
                                    if (state->canUnify(effect,cl->condition)) {
                                        FeatureTerm *u = simple_unification(effect,cl->condition,o,dm);
                                        if (u!=0) {
                                            threat = new SNLPThreat();
                                            threat->l = cl;
                                            threat->op = op;
                                            threat->condition = effect;
                                            delete u;
                                        }
                                    }
                                } // while
                            } else {
                                //                                printf("effect sort 3a: %s\n",effect->getSort()->get());
                                if (effect->getSort()==not_sort) effect = effect->featureValueQuick("predicate");
                                //                                printf("effect sort 3b: %s\n",effect->getSort()->get());
                                if (state->canUnify(effect,cl->condition)) {
                                    FeatureTerm *u = simple_unification(effect,cl->condition,o,dm);
                                    if (u!=0) {
                                        threat = new SNLPThreat();
                                        threat->l = cl;
                                        threat->op = op;
                                        threat->condition = effect;
                                        delete u;
                                    }
                                }
                            } // if
                        } else {
                            printf("SNLP_getSuccessors_step2: Error!!! SNLP only admits 'and' effects! (%s)\n",effect->getSort()->get());
                            return 0;
                        }                
                    } else {
                        //                        printf("SNLP_step2: %i does NOT theaten %i -> %i (%s) because of ordering\n",state->operators.Position(op),state->operators.Position(cl->op1),state->operators.Position(cl->op2),(cl->condition==0 ? "-":cl->condition->getSort()->get()));             
                    }
                } else {
                    //                    printf("SNLP_step2: %i does NOT theaten %i -> %i (%s) because its one of them\n",state->operators.Position(op),state->operators.Position(cl->op1),state->operators.Position(cl->op2),(cl->condition==0 ? "-":cl->condition->getSort()->get()));                                 
                }
            }
        }
    }
    
    if (threat==0) {
//        printf("SNLP_getSuccessors_step2: no threats, continuing...\n");
        List<FeatureTerm> a,b;
        successors->Add(state->clone_adding(0,dm,&a,&b));
        return successors;
    } else {
//        printf("SNLP_getSuccessors_step2: found a threat (%i threatens %i -> %i (%s))\n",state->operators.Position(threat->op),state->operators.Position(threat->l->op1),state->operators.Position(threat->l->op2),(threat->l->condition==0 ? "-":threat->l->condition->getSort()->get()));
        
        List<SNLPState> next_states;
        
        // op before the causal link:
        if (!state->operator_before(threat->l->op1,threat->op)) {
            List<FeatureTerm> l1,l2;
            
            SNLPState *s = state->clone_adding(0,dm,&l1,&l2);
            if (s!=0) {
                SNLPCausalLink *l = new SNLPCausalLink();
                l->op1 = threat->op->clone(&l1,&l2);
                l->op2 = threat->l->op1->clone(&l1,&l2);
                s->causal_links.Add(l);
                next_states.Add(s);
            }
        }
        
        // op after the causal link:
        if (!state->operator_before(threat->op,threat->l->op2)) {
            List<FeatureTerm> l1,l2;
            
            SNLPState *s = state->clone_adding(0,dm,&l1,&l2);
            if (s!=0) {
                SNLPCausalLink *l = new SNLPCausalLink();
                l->op1 = threat->l->op2->clone(&l1,&l2);
                l->op2 = threat->op->clone(&l1,&l2);
                s->causal_links.Add(l);
                next_states.Add(s);
            }
        }
        
        // separation constraint:
        {
            List<FeatureTerm> l1,l2;
            
            SNLPState *s = state->clone_adding(0,dm,&l1,&l2);
            if (s!=0) {
                Pair<FeatureTerm,FeatureTerm> *sc = new Pair<FeatureTerm,FeatureTerm>(threat->l->condition->clone(&l1,&l2),threat->condition->clone(&l1,&l2));
                if (sc->m_a->equivalents(sc->m_b)) {
                    delete sc;
                    delete s;
                } else {
                    s->separation_constraints.Add(sc);
                    next_states.Add(s);                
                }
            }
        }
        
        delete threat;
        
//        printf("SNLP_getSuccessors_step2: found %i ways to solve the threat\n",next_states.Length());
        
        while(!next_states.EmptyP()) {
            state = next_states.ExtractIni();
            
            // Calling step 2 again to see if there are more threats:
            successors_tmp = SNLP_getSuccessors_step2(state,dm,o);
            successors->AddAll(successors_tmp);
            successors_tmp->ExtractAll();
            delete successors_tmp;
            delete state;
        }
        
    }
    
	return successors;
} // SNLP_getSuccessors_step2


List<SNLPState> *SNLP_get_potentially_existing_satisfying_operators(SNLPState *state,Pair<FeatureTerm,FeatureTerm> *condition,FTKBase *dm,Ontology *o)
{
	List<SNLPState> *l = new List<SNLPState>;
    List<FeatureTerm> l2;
    FeatureTerm *op;
    FeatureTerm *g = condition->m_a;
    FeatureTerm *g_op = condition->m_b;
    
//    printf("SNLP_get_potentially_existing_satisfying_operators: %i operators to try...\n",state->operators.Length());

    // This code is wrong, what it should do is try different substitutions of parameters in the operator and see if they make the operator satisfy 'g':

	// Look in the existing operators:
    l2.Instance(state->operators);
    l2.Rewind();
    while(l2.Iterate(op)) {
        
        if (!op->same(g_op) && !state->operator_before(g_op,op)) {
            FeatureTerm *effect = op->featureValueQuick("effect");
            if (effect!=0 && effect->getSort()==and_sort) {
                effect = effect->featureValueQuick("predicates");                    
                
                if (effect->isSet()) {
                    List<FeatureTerm> effects;
                    effects.Instance(*(effect->set_values_quick()));
                    effects.Rewind();
                    while(effects.Iterate(effect)) {
                        List<UnificationResult> *ur_l = unification_with_bindings(effect,g,o,dm);
                        if (ur_l!=0) {                        
                            if (!ur_l->EmptyP()) {
                                UnificationResult *ur = ur_l->ExtractIni();
                                List<FeatureTerm> old_n,new_n;
                                UnificationBinding *b;
                                ur->m_bindings.Rewind();
                                while(ur->m_bindings.Iterate(b)) {
                                    old_n.Add(new FeatureTerm(b->m_f1));
                                    new_n.Add(new FeatureTerm(b->m_u));
                                    old_n.Add(new FeatureTerm(b->m_f2));
                                    new_n.Add(new FeatureTerm(b->m_u));
                                }
//                                printf("SNLP_get_potentially_existing_satisfying_operators: %i correspondences\n",old_n.Length());
/*                                
                                {
                                    char *s = op->toStringNOOS(dm);
                                    printf("SNLP_get_potentially_existing_satisfying_operators: Using operator:\n%s\n",s);
                                    delete []s;
                                }
*/                                
                                // add 's_add' to the plan, and insert causal link
                                SNLPState *state2 = state->clone_adding(0,dm,&old_n,&new_n);
                                delete ur;
                                
                                if (state2!=0) {
                                    // Add the causal link:
                                    state2->addCausalLink(state,op,g);
                                    
                                    // Remove the satisfied condition:
                                    state2->removeCondition(state,g);
                                    
                                    l->Add(state2);                                    
                                }
                            }
                            delete ur_l;
                        }
                    }
                } else {
                    List<UnificationResult> *ur_l = unification_with_bindings(effect,g,o,dm);
                    if (ur_l!=0) {                        
                        if (!ur_l->EmptyP()) {
                            UnificationResult *ur = ur_l->ExtractIni();
                            List<FeatureTerm> old_n,new_n;
                            UnificationBinding *b;
                            ur->m_bindings.Rewind();
                            while(ur->m_bindings.Iterate(b)) {
                                old_n.Add(new FeatureTerm(b->m_f1));
                                new_n.Add(new FeatureTerm(b->m_u));
                                old_n.Add(new FeatureTerm(b->m_f2));
                                new_n.Add(new FeatureTerm(b->m_u));
                            }
//                            printf("SNLP_get_potentially_existing_satisfying_operators: %i correspondences\n",old_n.Length());
/*                            
                            {
                                char *s = op->toStringNOOS(dm);
                                printf("SNLP_get_potentially_existing_satisfying_operators: Using operator:\n%s\n",s);
                                delete []s;
                            }
*/                            
                            // add 's_add' to the plan, and insert causal link
                            SNLPState *state2 = state->clone_adding(0,dm,&old_n,&new_n);
                            delete ur;
                            
                            if (state2!=0) {
                                // Add the causal link:
                                state2->addCausalLink(state,op,g);
                                
                                // Remove the satisfied condition:
                                state2->removeCondition(state,g);
                                
                                l->Add(state2);
                            }
                        }
                        delete ur_l;
                    }
                }
            }
        } // if (operator_before)
    } // while

    
	return l;
} /* SNLP_get_potentially_existing_satisfying_operators */ 


List<SNLPState> *SNLP_get_potentially_new_satisfying_operators(SNLPState *state,FeatureTerm *g,FTKBase *dm,Ontology *o)
{
	List<SNLPState> *l = new List<SNLPState>;
    
	List<FeatureTerm> *operators = dm->SearchFT(operator_sort);
	FeatureTerm *op;

//    printf("SNLP_get_potentially_new_satisfying_operators: %i operators to try...\n",operators->Length());
    
	while(!operators->EmptyP()) {
        op = operators->ExtractIni();
        FeatureTerm *effect = op->featureValueQuick("effect");
        if (effect->getSort()==and_sort) {
            effect = effect->featureValueQuick("predicates");                    
            if (effect->isSet()) {
                List<FeatureTerm> effects;
                effects.Instance(*(effect->set_values_quick()));
                effects.Rewind();
                while(effects.Iterate(effect)) {
                    List<UnificationResult> *ur_l = unification_with_bindings(effect,g,o,dm);
                    if (ur_l!=0) {                        
                        if (!ur_l->EmptyP()) {
                            UnificationResult *ur = ur_l->ExtractIni();
                            List<FeatureTerm> old_n,new_n;
                            UnificationBinding *b;
                            ur->m_bindings.Rewind();
                            while(ur->m_bindings.Iterate(b)) {
                                old_n.Add(new FeatureTerm(b->m_f1));
                                new_n.Add(new FeatureTerm(b->m_u));
                                old_n.Add(new FeatureTerm(b->m_f2));
                                new_n.Add(new FeatureTerm(b->m_u));
                            }
//                            printf("SNLP_get_potentially_new_satisfying_operators: %i correspondences\n",old_n.Length());
                            
                            FeatureTerm *clone = op->clone(&old_n,&new_n);
/*                            
                            {
                                char *s = clone->toStringNOOS(dm);
                                printf("SNLP_get_potentially_new_satisfying_operators: Using operator:\n%s\n",s);
                                delete []s;
                            }
*/                            
                            // add 's_add' to the plan:
                            SNLPState *state2 = state->clone_adding(clone,dm,&old_n,&new_n);
                            delete clone;
                            delete ur;
                            
                            if (state2!=0) {
                                // Add the causal link:
                                state2->addCausalLink(state,0,g);

                                // Remove the satisfied condition:
                                state2->removeCondition(state,g);
                                                            
                                l->Add(state2);
                            }
                        }
                        delete ur_l;
                    }
                } // while
            } else {
                List<UnificationResult> *ur_l = unification_with_bindings(effect,g,o,dm);
                if (ur_l!=0) {
                    if (!ur_l->EmptyP()) {
                        UnificationResult *ur = ur_l->ExtractIni(); // ur_l MUST have at least one result, otherwise, there's some bug
                        List<FeatureTerm> old_n,new_n;
                        UnificationBinding *b;
                        ur->m_bindings.Rewind();
                        while(ur->m_bindings.Iterate(b)) {
                            old_n.Add(new FeatureTerm(b->m_f1));
                            new_n.Add(new FeatureTerm(b->m_u));
                            old_n.Add(new FeatureTerm(b->m_f2));
                            new_n.Add(new FeatureTerm(b->m_u));
                        }
                        
                        FeatureTerm *clone = op->clone(&old_n,&new_n);
                        clone->setName(0);
/*                        
                        {
                            char *s = clone->toStringNOOS(dm);
                            printf("SNLP_get_potentially_new_satisfying_operators: Using operator:\n%s\n",s);
                            delete []s;
                        }
*/                        
                        // add 's_add' to the plan, and insert causal link
                        SNLPState *state2 = state->clone_adding(clone,dm,&old_n,&new_n);
                        delete clone;
                        delete ur;
                        
                        if (state2!=0) {
                            // Add the causal link:
                            state2->addCausalLink(state,0,g);
                            
                            // Remove the satisfied condition:
                            state2->removeCondition(state,g);
                            
                            l->Add(state2);
                        }
                    }
                    delete ur_l;
                }
            }        
        }
//        printf("SNLP_get_potentially_new_satisfying_operators: %i remaining...\n",operators->Length());
	} // while 
    
	delete operators;
        
	return l;
} /* SNLP_get_potentially_new_satisfying_operators */ 




List<FeatureTerm> *SNLPState::get_linearization(void)
{
	List<FeatureTerm> *l = new List<FeatureTerm>;
    List<FeatureTerm> l2;
    FeatureTerm *op;
    
    l2.Instance(operators);
	l2.Rewind();
    while(l2.Iterate(op)) l->Add(new FeatureTerm(op));

    {
		LLink<FeatureTerm> *l1,*l2;
		FeatureTerm* tmp;
		bool change;
                
		do{
			change=false;
/*            
            {
                FeatureTerm *f;
                List<FeatureTerm> l_test;
                l_test.Instance(*l);
                l_test.Rewind();
                printf("[ ");
                while(l_test.Iterate(f)) {
                    printf("%i ",operators.Position(f));
                }
                printf("]\n");
            }
*/            
            l->Rewind();
			l1=l->GetPos();
            while(l1!=0) {
                l2=l1->Getnext();
                while(l2!=0) {
                    if (operator_before(l2->GetObj(),l1->GetObj())) {
                        FeatureTerm *tmp = l2->GetObj();
                        l2->SetObj(l1->GetObj());
                        l1->SetObj(tmp);
                        change = true;
                    }
                    
                    l2=l2->Getnext();
                }
                l1=l1->Getnext();
            }
            
		}while(change);
	} /* Sort */
    
	return l;
} /* SNLPState::get_linearization */ 


bool SNLPState::operator_before(FeatureTerm *op1,FeatureTerm *op2)
{
    FeatureTerm *original_op2 = op2;
    List<FeatureTerm> already_added;
    List<FeatureTerm> before_operators;
    List<SNLPCausalLink> l;
    SNLPCausalLink *link;
    
//    printf("operator_before: %i < %i?\n",operators.Position(op1),operators.Position(op2));

    l.Instance(causal_links);
    
    before_operators.Add(op2);
    already_added.Add(op2);
    while(!before_operators.EmptyP()) {
        op2 = before_operators.ExtractIni();
//        printf("checking: %i\n",operators.Position(op2));

        l.Rewind();
        while(l.Iterate(link)) {
            if (op2->same(link->op2)) {
                
                if (original_op2->same(link->op1)) {
                    before_operators.ExtractAll();
                    already_added.ExtractAll();
                    printf("operator_before: ERROR!!!!!!!!!!!! Circularity in dependencies!!!\n");
                    return false;
                }
                
                if (op1->same(link->op1)) {
                    before_operators.ExtractAll();
                    already_added.ExtractAll();
//                    printf("operator_before: en!!\n");
                    return true;
                } // if
                
                if (!already_added.MemberP(link->op1)) {
                    before_operators.Add(link->op1);
                    already_added.Add(link->op1);
                }
            }
        }
    }
    
    already_added.ExtractAll();
    
//    printf("operator_before: no...\n");

	return false;
} // operator_before


SNLPState *SNLPState::clone_adding(FeatureTerm *op,FTKBase *dm,List<FeatureTerm> *old_c,List<FeatureTerm> *new_c)
{    
//    printf("clone_adding in ...\n");
    SNLPState *s = new SNLPState();
    
    FeatureTerm *f1,*f2,*f3;
    Pair<FeatureTerm,FeatureTerm> *p;
    List<Pair<FeatureTerm,FeatureTerm> > l1;
    l1.Instance(open_conditions);
    l1.Rewind();
    while(l1.Iterate(p)) {
        f1 = (p->m_a==0 ? 0:p->m_a->clone(old_c,new_c));
        f2 = (p->m_b==0 ? 0:p->m_b->clone(old_c,new_c));
        
        s->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(f1,f2));
    }

//    printf("clone_adding in ...1\n");
    
    List<FeatureTerm> l2;
    l2.Instance(operators);
    l2.Rewind();
    while(l2.Iterate(f1)) {
        f1 = f1->clone(old_c,new_c);        
        s->operators.Add(f1);
    }
    
//    printf("clone_adding in ...2\n");

    // add new operator
    if (op!=0) {
        f1 = op->clone(old_c,new_c);
        s->operators.Add(f1);
        
        // Add all the preconditions to open conditions:
        FeatureTerm *preconditions = f1->featureValueQuick("precondition");
        if (preconditions->getSort()->is_a(and_sort)) {
            FeatureTerm *f = preconditions->featureValueQuick("predicates");
            if (f->isSet()) {
                List<FeatureTerm> l;
                l.Instance(*(f->set_values_quick()));
                l.Rewind();
                while(l.Iterate(f)) {
                    s->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(new FeatureTerm(f),new FeatureTerm(f1)));
                } // while
            } else {
                s->open_conditions.Add(new Pair<FeatureTerm,FeatureTerm>(new FeatureTerm(f),new FeatureTerm(f1)));
            } // if
        } else {
            printf("clone_adding: SNLP only supports 'and' goal state structures!\n");
        } // if
        
        // Constrain that it has to be after the initial state and before the final state:
        {
            SNLPCausalLink *l = new SNLPCausalLink();
            l->op1 = new FeatureTerm(s->operators[0]);
            l->op2 = new FeatureTerm(f1);
            l->condition = 0;
            s->causal_links.Add(l);
            
            l = new SNLPCausalLink();
            l->op1 = new FeatureTerm(f1);
            l->op2 = new FeatureTerm(s->operators[1]);
            l->condition = 0;
            s->causal_links.Add(l);
        }
    }
    
//    printf("clone_adding in ...3\n");

    SNLPCausalLink *cl;
    List<SNLPCausalLink> l3;
    l3.Instance(causal_links);
    l3.Rewind();
    while(l3.Iterate(cl)) {
        f1 = (cl->op1==0 ? 0:cl->op1->clone(old_c,new_c));
        f2 = (cl->op2==0 ? 0:cl->op2->clone(old_c,new_c));
        f3 = (cl->condition==0 ? 0:cl->condition->clone(old_c,new_c));
        
        cl = new SNLPCausalLink();
        cl->op1 = f1;
        cl->op2 = f2;
        cl->condition = f3;
        
        s->causal_links.Add(cl);
    }
    
//    printf("clone_adding in ...4\n");

    l1.Instance(separation_constraints);
    l1.Rewind();
    while(l1.Iterate(p)) {
        f1 = p->m_a->clone(old_c,new_c);
        f2 = p->m_b->clone(old_c,new_c);
        
        s->separation_constraints.Add(new Pair<FeatureTerm,FeatureTerm>(f1,f2));
                
        if (f1->equivalents(f2)) {
            printf("clone_adding: ERROR!!!! separation constraints not satisfied!!!!!\n");
            delete s;
            return 0;
        }       
    }
         
//    printf("clone_adding: %i bindings. Done\n",old_c->Length());
    
    return s;
} // SNLPState::clone_adding


void SNLPState::printStateNice(FTKBase *dm)
{
    /*
     List<Pair<FeatureTerm,FeatureTerm> > open_conditions;	// List of unsatisfied conditions of any operator(or final state) <Condition,Operator>
     List<FeatureTerm> operators;
     List<SNLPCausalLink> causal_links;
     List<Pair<FeatureTerm,FeatureTerm> > separation_constraints;     
     */
    
    printf("SNLPState: -----------------------------------------------\n");
    printf("  open conditions: (%i)\n",open_conditions.Length());
    {
        char tmp[8];
        Pair<FeatureTerm,FeatureTerm> *c;
        List<Pair<FeatureTerm,FeatureTerm> > l;
        l.Instance(open_conditions);
        l.Rewind();
        while(l.Iterate(c)) {
            if (c->m_b==0) {
                strcpy(tmp,"end");
            } else {
                sprintf(tmp,"%i",operators.Position(c->m_b));
            }
            printf("    %s of %s\n",c->m_a->getSort()->get(),tmp);
        }
    }
    printf("  operators (%i):\n",operators.Length());
    {
        int i = 0;
        FeatureTerm *f;
        List<FeatureTerm> l;
        l.Instance(operators);
        l.Rewind();
        while(l.Iterate(f)) {
            printf("    %i - ",i);
            printOperatorNice(f,dm);
            i++;
        }
    }
    printf("  causal links (%i):\n",causal_links.Length());
    {
        char tmp[8],tmp2[8];
        SNLPCausalLink *c;
        List<SNLPCausalLink > l;
        l.Instance(causal_links);
        l.Rewind();
        while(l.Iterate(c)) {
            if (c->op1==0) {
                strcpy(tmp,"start");
            } else {
                sprintf(tmp,"%i",operators.Position(c->op1));
            }
            if (c->op2==0) {
                strcpy(tmp2,"end");
            } else {
                sprintf(tmp2,"%i",operators.Position(c->op2));
            }
            if (c->condition == 0) {
                printf("    %s -> %s (-)\n",tmp,tmp2);                
            } else {
                printf("    %s -> %s (%s)\n",tmp,tmp2,c->condition->getSort()->get());
            }
        }
    }
    printf("  separation constraints: %i\n",separation_constraints.Length());
    {
        List< Pair<FeatureTerm,FeatureTerm> > l1;
        Pair<FeatureTerm,FeatureTerm> *p;
        l1.Instance(separation_constraints);
        l1.Rewind();
        while(l1.Iterate(p)) {
            printf("    %p (%s) != %p (%s)\n",p->m_a->get_content(),p->m_a->getSort()->get(),p->m_b->get_content(),p->m_b->getSort()->get());                
            if (p->m_a->equivalents(p->m_b)) {
                printf("printStateNice: ERROR!!!! separation constraints not satisfied!!!!! (1)\n");
            }
        }
    }
        
    printf("-----------------------------------------------\n");
}

void SNLPState::addCausalLink(SNLPState *oldState,FeatureTerm *oldOp,FeatureTerm *oldCondition)
{
    Pair<FeatureTerm,FeatureTerm> *oc;
    List< Pair<FeatureTerm,FeatureTerm> > l;
    FeatureTerm *oldOp2 = 0;
    int pos = 0,found = -1;
    
    l.Instance(oldState->open_conditions);
    l.Rewind();
    while(found==-1 && l.Iterate(oc)) {
        if (*(oc->m_a)==*oldCondition) {
            found = pos;   
            oldOp2 = oc->m_b;
        }
        pos++;
    }
    
//    printf("addCausalLink: found = %i\n",found);
    
    FeatureTerm *newOp1,*newOp2,*newCondition;
    
    if (oldOp==0) {
        newOp1 = operators[operators.Length()-1];
    } else {
        newOp1 = operators[oldState->operators.Position(oldOp)];
    }

    if (oldOp2==0) {
        newOp2 = 0;
    } else {
        newOp2 = operators[oldState->operators.Position(oldOp2)];
    }
    
    newCondition = open_conditions[found]->m_a;
    
    SNLPCausalLink *link = new SNLPCausalLink();
    link->op1 = (newOp1 == 0 ? 0:new FeatureTerm(newOp1));
    link->op2 = (newOp2 == 0 ? 0:new FeatureTerm(newOp2));
    link->condition = new FeatureTerm(newCondition);
    
    causal_links.Add(link);
}


void SNLPState::removeCondition(SNLPState *oldState,FeatureTerm *oldCondition)
{
    Pair<FeatureTerm,FeatureTerm> *oc;
    List< Pair<FeatureTerm,FeatureTerm> > l;
    int pos = 0,found = -1;
    
    l.Instance(oldState->open_conditions);
    l.Rewind();
    while(found==-1 && l.Iterate(oc)) {
        if (*(oc->m_a)==*oldCondition) {
            found = pos;   
        }
        pos++;
    }
    
//    printf("removeCondition: found = %i\n",found);
    
    oc = open_conditions[found];
    open_conditions.DeletePosition(found);
    delete oc;
}

bool SNLPState::canUnify(FeatureTerm *g1, FeatureTerm *g2)
{
    List< Pair<FeatureTerm,FeatureTerm> > l;
    Pair<FeatureTerm,FeatureTerm> *sc;
    
    l.Instance(separation_constraints);
    l.Rewind();
    while(l.Iterate(sc)) {
        if ((sc->m_a->same(g1) && sc->m_b->same(g2)) ||
            (sc->m_a->same(g2) && sc->m_b->same(g1))) {
            return false;
        }
    }

    return true;
}


bool HSNLPSortOrder(SNLPState *o1,SNLPState *o2)
{
	int l1 = o1->open_conditions.Length();
    int l2 = o2->open_conditions.Length();
    
    return l1<=l2;
/*    
    if (l1<l2) return true;
    if (l1==l2) {
        return (o1->operators.Length()<=o2->operators.Length());
    }
    
    return false;
 */
} /* HSNLPSortOrder */ 
